#/usr/bin/env python
import roslib; roslib.load_manifest('stack_objects')
import rospy

from geometry_msgs.msg import PointStamped
from geometry_msgs.msg import PoseStamped

from pr2_tasks.tasks import Tasks
from pr2_tasks.pickplace import PickPlace
from pr2_tasks.tableware_detection import TablewareDetection
from pr2_tasks.pickplace_definitions import PlaceGoal
from pr2_tasks.pickup_tableware import PickupTableware

from pr2_python import gripper
from pr2_python.arm_mover import ArmMover
from pr2_python import base
from pr2_python.world_interface import WorldInterface
from pr2_python.planning_scene_interface import get_planning_scene_interface
from pr2_python.arm_mover import ArmMover
from pr2_python.controller_manager_client import ControllerManagerClient
from pr2_python import geometry_tools
import pr2_python.transform_listener as tl

import copy
import ee_cart_imped_action

from PlatePicker import *
from Room3 import *


class Placer:
    def set_it_there(self, arm_name, place_pose_stamped, table_name=''):
        room = Room3()
        room.move_arms_back()
        #are we holding anything?
        place_obj_id = None
        psi = get_planning_scene_interface()
        psi.reset()
        wi = WorldInterface()
        attached_objects = psi.attached_collision_objects()

        for ao in attached_objects:
            if ao.link_name == psi.hands[arm_name].attach_link:
                place_obj_id = ao.object.id
                rospy.loginfo('Found attached object '+place_obj_id)
                break
        if place_obj_id == None:
            rospy.logwarn('Place: we do not think we are holding anything.  Will assume place_pose is wrist pose')
            place_obj_id = ''

        rospy.loginfo('Moving to manipulable pose')
        map_pose = tl.transform_pose(wi.world_frame, place_pose_stamped.header.frame_id, place_pose_stamped.pose)
        manipulable_poses = []

        b = base.Base()
        det = TablewareDetection()

        searchplace = []
        for x in [0.01, 0.02, -0.01, -0.02]:
          for y in [0.05, 0.1]:
            for z in [0.02, 0.04]:
              for a in [0, 3.14/2, 3.14/4, -3.14/2]:
                searchplace.append((x, y, z, a))
        pickplace = PickPlace(search_place = searchplace)

        for (x, y) in [(0.0, 0.0), (-0.2, 0.0), (0.0, -0.2), (0.2, 0.0), (0.0, 0.2)]:
            manipulable_poses += b.get_base_poses(map_pose.position.x+x, map_pose.position.y+y, 
                                                          map_pose.position.z)
        if not manipulable_poses:
            return
        rospy.loginfo('Will try '+str(len(manipulable_poses))+' base poses')

        for pose in manipulable_poses:
            angles = geometry_tools.quaternion_to_euler(pose.pose.orientation)
            rospy.loginfo('Moving to manipulable pose')
            try:
                b.move_to(pose.pose.position.x, pose.pose.position.y, angles[2])
            except:
                rospy.logerr('Base move failed')
                continue
            if table_name == '':
                #see if we can find a table to place on
                rospy.loginfo('Attempting to find a table to place on')
                try:
                    head_point = PointStamped()
                    head_point.header = place_pose_stamped.header
                    head_point.point = place_pose_stamped.pose.position
                    det_res = detector.detect_objects(add_objects_to_map=False, reset_collision_objects=False,
                                                           table_name='place_table', point_head_at=head_point)
                    table_name = det_res.table_name
                except:
                    rospy.logwarn('Unable to find place table.')
            place_goal = PlaceGoal(arm_name, [place_pose_stamped], collision_object_name=place_obj_id, 
                                   collision_support_surface_name=table_name, allow_gripper_support_collision=True)
            try:
                place_result = pickplace.place(place_goal)
            except:
                rospy.logerr('Place failed with error')
                room.move_arms_back()
                continue
            rospy.loginfo('Successful go and place')
            return place_result

